home *** CD-ROM | disk | FTP | other *** search
/ Cream of the Crop 26 / Cream of the Crop 26.iso / os2 / octa209s.zip / octave-2.09 / scripts / control / dlqe.m < prev    next >
Text File  |  1996-07-15  |  2KB  |  72 lines
  1. ## Copyright (C) 1996 John W. Eaton
  2. ##
  3. ## This file is part of Octave.
  4. ##
  5. ## Octave is free software; you can redistribute it and/or modify it
  6. ## under the terms of the GNU General Public License as published by
  7. ## the Free Software Foundation; either version 2, or (at your option)
  8. ## any later version.
  9. ##
  10. ## Octave is distributed in the hope that it will be useful, but
  11. ## WITHOUT ANY WARRANTY; without even the implied warranty of
  12. ## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13. ## General Public License for more details.
  14. ##
  15. ## You should have received a copy of the GNU General Public License
  16. ## along with Octave; see the file COPYING.  If not, write to the Free
  17. ## Software Foundation, 59 Temple Place - Suite 330, Boston, MA
  18. ## 02111-1307, USA.
  19.  
  20. ## Usage: [l, m, p, e] = dlqe (A, G, C, SigW, SigV {,Z})
  21. ##
  22. ## Linear quadratic estimator (Kalman filter) design for the
  23. ## discrete time system
  24. ##
  25. ##  x[k+1] = A x[k] + B u[k] + G w[k]
  26. ##    y[k] = C x[k] + D u[k] + w[k]
  27. ##
  28. ## where w, v are zero-mean gaussian noise processes with respective
  29. ## intensities SigW = cov (w, w) and SigV = cov (v, v).
  30. ##
  31. ## Z (if specified) is cov(w,v); otherwise cov(w,v) = 0.
  32. ##
  33. ## Observer structure is
  34. ##     z[k+1] = A z[k] + B u[k] + k(y[k] - C z[k] - D u[k]).
  35. ##
  36. ## Returns:
  37. ##
  38. ##   l = observer gain, (A - A L C) is stable
  39. ##   m = Ricatti equation solution
  40. ##   p = the estimate error covariance after the measurement update
  41. ##   e = closed loop poles of (A - A L C)
  42.  
  43. ## Author: A. S. Hodel <scotte@eng.auburn.edu>
  44. ##         R. Bruce Tenison <btenison@eng.auburn.edu>
  45. ## Created: August 1993
  46. ## Adapted-By: jwe
  47.  
  48. function [l, m, p, e] = dlqe (a, g, c, sigw, sigv, zz)
  49.  
  50.   if (nargin != 5 && nargin != 6)
  51.     error ("dlqe: invalid number of arguments");
  52.   endif
  53.  
  54.   ## The problem is dual to the regulator design, so transform to lqr
  55.   ## call.
  56.  
  57.   if (nargin == 5)
  58.     [k, p, e] = dlqr (a', c', g*sigw*g', sigv);
  59.     m = p';
  60.     l = (m*c')/(c*m*c'+sigv);
  61.   else
  62.     [k, p, e] = dlqr (a', c', g*sigw*g', sigv, g*zz);
  63.     m = p';
  64.     l = (m*c'+a\g)/(c*m*c'+sigv);
  65.     a = a-g*t/sigv*c;
  66.     sigw = sigw-t/sigv;
  67.   endif
  68.  
  69.   p = a\(m-g*sigw*g')/a';
  70.  
  71. endfunction
  72.